
import time
# 引入mqtt包
import paho.mqtt.client as mqtt
# 使用独立线程运行
from threading import Thread

import json

from telemetrix import telemetrix



from pymata4 import pymata4

CURRENT_LOCATION = [0,0]   #当前坐标

COMPLETED_FLAG = 0   #是否完成 0 未完成 1 完成

board = telemetrix.Telemetrix(com_port='/dev/ttyUSB0')
board1 = pymata4.Pymata4(com_port='/dev/ttyACM0')
ENABLE_PIN =  8
board.set_pin_mode_digital_output(pin_number=ENABLE_PIN)
board.digital_write(pin=ENABLE_PIN,value=0)






keyBoard = {        "192":[0,0],"49":[0,94],"50":[0,94*2],"51":[0,94*3],
                    "52":[0,94*4],"53":[0,94*5],"54":[0,94*6],"55":[0,94*7],
                    "56":[0,94*8],"57":[0,94*9],"58":[0,94*10],"48":[0,94*11],
                    "189":[0,94*12],"187":[0,94*13],"8":[0,94*14],

                    "9":[94,20],"81":[94,130],"87":[94,130+94],"69":[94,130+94*2],
                    "82":[94,130+94*3],"84":[94,130+94*4],"89":[94,130+94*5],
                    "85":[94,130+94*6],"73":[94,130+94*7],"79":[94,130+94*8],
                    "80":[94,130+94*9],"219":[94,130+94*10],"221":[94,130+94*11],
                    "220":[94,170+94*12],

                    "20": [94*2, 35],"65": [94*2, 160],"83": [94*2, 160+94],"68": [94*2, 160+94*2],
                    "70": [94*2, 160+94*3],"71": [94*2, 160+94*4],"72": [94*2, 160+94*5],"74": [94*2, 160+94*6],
                    "75": [94*2, 160+94*7],"76": [94*2, 160+94*8],"186": [94*2, 160+94*9],"222": [94*2, 160+94*10],
                    "1301": [94*2, 210+94*11],

                    "90": [94*3, 210],"90": [94*3, 210],"88": [94*3, 210+94*1],"67": [94*3, 210+94*2],"86": [94*3, 210+94*3],
                    "66": [94*3, 210+94*4],"78": [94*3, 210+94*5],"77": [94*3, 210+94*6],"188": [94*3, 210+94*7],
                    "190": [94*3, 210+94*8],"191": [94*3, 210+94*9],"1601": [94*3, 210+94*11],

                    "32": [94*4, 630],



            }

print(keyBoard)


def i2cCallPwd(my_board,pin):
    my_board.set_pin_mode_i2c()
    my_board.i2c_write(pin, [0,0])

def i2cCallstepper(my_board,pin,xstepper,ystepper):
    my_board.set_pin_mode_i2c()
    my_board.i2c_write(pin, [xstepper, ystepper])

def servo(my_board, pin, angle ):
    my_board.set_pin_mode_servo(pin)
    my_board.servo_write(pin, angle)




# def servo(pin,angle):
#     board.set_pin_mode_servo(pin)
#     board.servo_write(pin,angle)
#     # board.servo_detach(pin)


def formKeyCodeGetPosition(keyCode):
    absoluteAddress = keyBoard[keyCode]
    global  CURRENT_LOCATION
    global  COMPLETED_FLAG
    COMPLETED_FLAG = 0
    stepperX = CURRENT_LOCATION[0] - absoluteAddress[0]
    stepperY = CURRENT_LOCATION[1] - absoluteAddress[1]

    if stepperY != 0:
        runStepper(stepperY,stepperY)
        while COMPLETED_FLAG == 0:
            time.sleep(.2)
    COMPLETED_FLAG = 0
    if stepperX != 0:
        runStepper(-stepperX,stepperX)
        while COMPLETED_FLAG == 0:
            time.sleep(.2)

    servo(board1, 9, 220)

    time.sleep(.2)

    servo(board1, 9, 120)


    CURRENT_LOCATION = absoluteAddress
    print(CURRENT_LOCATION)
    print(stepperX,stepperY)




def the_callback(data):
    global  COMPLETED_FLAG
    date = time.strftime('%Y-%m-%d %H:%M:%S', time.localtime(data[2]))
    print(f'Motor {data[1]} runSpeedToPosition motion completed at: {date}.')
    COMPLETED_FLAG = 1


motor = board.set_pin_mode_stepper(interface=1, pin1=2, pin2=5)
motor1 = board.set_pin_mode_stepper(interface=1, pin1=3, pin2=6)

def runStepper(xsteppet,ysteppet):
    board.stepper_set_current_position(motor,0)
    board.stepper_set_current_position(motor1,0)
    board.stepper_set_max_speed(motor, 800)
    board.stepper_set_max_speed(motor1, 800)
    board.stepper_move_to(motor, xsteppet)
    board.stepper_move_to(motor1, ysteppet)
    board.stepper_set_speed(motor1, 100)
    # run the motor
    board.stepper_run_speed_to_position(motor, completion_callback=the_callback)
    board.stepper_run_speed_to_position(motor1, completion_callback=the_callback)









# 建立mqtt连接
def on_connect(client, userdata, flag, rc):
    print("Connect with the result code " + str(rc))
    client.subscribe('mqtt/4b-01', qos=2)


# 接收、处理mqtt消息
def on_message(client, userdata, msg):
    out = str(msg.payload.decode('utf-8'))
    print(msg.topic)


    mqttJsonMessage = json.loads(out)
    print(mqttJsonMessage)
    if mqttJsonMessage["gear"] == "stepper":
        formKeyCodeGetPosition(mqttJsonMessage["keycode"])

    if mqttJsonMessage["gear"] == "steeringEngine":
        servo(board1, int(mqttJsonMessage["pin"]), int(mqttJsonMessage["start"]))


    if mqttJsonMessage["gear"] == "electromagnet":
        i2cCallPwd(board1, int(mqttJsonMessage["pin"]))



# mqtt客户端启动函数
def mqttfunction():
    global client
    # 使用loop_start 可以避免阻塞Django进程，使用loop_forever()可能会阻塞系统进程
    # client.loop_start()
    # client.loop_forever() 有掉线重连功能
    client.loop_forever(retry_first_connection=True)


client = mqtt.Client(client_id="4b-01", clean_session=False)


# 启动函数
def mqtt_run():
    client.on_connect = on_connect
    client.on_message = on_message
    # 绑定 MQTT 服务器地址
    broker = '47.102.200.101'
    # MQTT服务器的端口号
    client.connect(broker, 1883, 60)
    client.username_pw_set('admin', 'qmy980902')
    client.reconnect_delay_set(min_delay=1, max_delay=2000)
    # 启动
    mqttthread = Thread(target=mqttfunction)
    mqttthread.start()




if __name__ == "__main__":
    mqtt_run()
